%data = load('test1.txt');
%data = load('ranging_data.txt');
data = load('result.txt');
%data = data';
pos = data(:,1:3);
%pos = data(2:4,:);
%Angle data
%a = 0:360;
%a = (a/2)*pi/180;

figure(1);
hold off;
plot(0,0,'rx');

hold on;
axis "equal";
map = load('map.txt');
plotLineMap(map',0,'r')
grid on;
axis([-4 2 -1 3]);

vr = load('virtualscan.txt');

for i=1:10:size(data,1);

% Range measurement
r = data(i,:);
r = r(4:184);
a = data(i,:);
a = a(185:end);
%r = r(5:end);
p = pos(i,:);

[v,ind]=find(r>80);
r(ind)=0;

x = r.*cos(a + p(3)) + p(1);
y = r.*sin(a + p(3)) + p(2);

%vx = vr.*cos(a);
%vy = vr.*sin(a);
%plot(vx,vy,'gx')
plot(x(1:2:end),y(1:2:end),'.b');
plot(p(1),p(2),'rx');
%sleep(0.15)
end;
print('j2b2_mcl_result.png','-dpng')